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Abstract 

A method of dealing with singularities and joint 
limits in the inverse kinematics for both redundant and 
non-redundant serial-link manipulators is presented. 
The method uses damped least squares with dynamic 
weighting for the approximate solution of the inverse 
Jacobian problem. Damped least squares has become a 
popular approach for dealing with singularities. The 
method presented extends the utility of damped least 
squares by incorporating dynamic weighting matrices 
within its formulation. This allows specific joints to be 
targeted in the minimization of the joint differential 
vector. An efficient algorithm is given for the solution 
of the weighted damped least squares problem. This 
algorithm is implemented, along with an algorithm to set 
the weights, for a six d.o.f. telemanipulator slave. A 
solution that is approximate in the task space and that is 
physically realizable in the joint space is obtained at or 
near singularities and/or joint limits. Away from 
singularities and joint limits an exact solution is 
obtained. The results are a well behaved slave 
manipulator under teleoperational control even when the 
slave is at or near singularities and/or when unreachable 
configurations are commanded. 

1 Introduction 

The inverse kinematics problem can be stated as 
follows: given a desired position and orientation of the 
end effector of a manipulator find a joint configuration 
that satisfies it. This problem is central in the control of 
robot manipulators. Any time the motions of a 
manipulator are described in a general space such as a 
Cartesian space, the inverse kinematics must be solved. 
In order to avoid this, manipulators are often controlled 
by describing the motions only in the joint space. This is 
done, however, with a great loss of generality. For 
serial-link manipulators the inverse kinematics problem 
is complicated by non-linearities, singularities, 
unreachable configurations, multiple solutions, and even 
infinite solutions in the case of a redundant manipulator. 
The nonlinearities can be avoided by calculating the 
inverse kinematics iteratively using the Jacobian of the 


manipulator. Redundancies, while complicating the 
solutions, are actually utilized to satisfy some criteria 
that is secondary to the motion of the end effector. This 
is a large body of research. The focus of this paper is in 
dealing with singularities and unreachable 
configurations. 

The methods discussed here utilize the damped 
least squares inverse of the Jacobian, which has been 
proposed by many researchers for the inverse kinematics 
problem. 1 - 2 * 3 ’ 4 - 5 This inverse has the benefit of being 
compatible with solutions based on the pseudoinverse 
which has become a very popular method of calculating 
the inverse Jacobian. The pseudoinverse has become 
popular for many reasons including the utilization of 
redundancy. 6 - 7 - 8 - 9 The use of damped least squares 
results in an approximate solution with a decrease in the 
size of the solution vector. This is beneficial in 
controlling the large joint rates resulting from exact 
solutions near singularities. The addition of dynamic 
weighting matrices in the damped least squares solution 
is proposed in this paper, to increase its utility. Using 
weighting matrices the damped least squares solution is 
extended to methods of dealing with unreachable 
configurations caused by joint limits. The previous 
research with damped least squares has dealt mainly 
with singularities. The dynamic weights can also be 
used to target the problem joints, of a particular 
singularity in reducing the size of the joint space solution 
vector. 

For any serial-link manipulator a particular 
configuration of the joints corresponds to a unique 
position and orientation of the end effector in Cartesian 
space. This relationship is described by the forward 
kinematics function of the manipulator. The methods 
used to develop this function are well established. The 
position and orientation, or the task space variable, 
x g R m (generally m=6), of the end effector is described 
as a function of the joint space variable, q e R n , by the 
nonlinear forward kinematics equation, 

* = A(q) (1) 
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The differential relationship of x and q is described 
using the following linear equation: 

8x = J(q)8q where J(q)g (2) 

dq/dt 

In equation (2) J(q) is the m x n manipultor Jacobian 
matrix. General methods for the development of the 
forward kinematics function and the Jacobian of a 
manipulator can be found in any introductory text on 
robotics such as Craig 10 , Paul 11 , or Koivo 12 . (Note: 
Hereafter the functional dependence of J on q will be 
dropped and assumed to be understood.) 

The inverse kinematics problem almost always 
reduces to that of solving equation (1) for q or equation 
(2) for 8q in an iterative scheme to find q. Analytical 
solutions of equation (1) are known only for a few simple 
non-redundant (m=n) manipulator geometries. The six 
degree of freedom (m=n=6) manipulator geometries for 
which these solutions exist were clarified by Pieper 13 . 
Iterative inverse kinematics schemes can be used by 
solving equation (2). This can be done off line or it can 
be done on line within the control system of the 
manipulator, using each iteration to calculate the joint 
control law. An example of on-line iterative inverse 
kinematics is resolved rate control. 14 Nearly all of the 
contemporary research dealing with inverse kinematics 
solutions is devoted to finding a solution or an 
approximate solution to equation (2). This is true for 
three reasons: 1) there are many manipulators for which 
an analytical solution to (1) does not exist; 2) the 
nonlinearities of equation (1) impede the development of 
general methods for a numerical solution procedure; 3) 
general methods for dealing with the other complications 
of inverse kinematics can be incorporated in the solution 
of (2). Solutions to equation (2) are commonly found 
by solving a linear system of equations for 8q using some 
well established method, such as Gaussian elimination. 
In the non-redundant, exact case these solutions are 
described using the equation 

8q = J" 1 8x (3) 

This equation may be generalized to include redundant 
manipulators and/or approximate solutions using the 
following equation: 

8q = J # 8x (4) 


In this equation J # is a some type of inverse of the 
Jacobian matrix. If m=n then J # is likely to be J -1 , 
whereas for redundant manipulators (n>m) J # might be 
the Moore-Penrose pseudoinverse 15 , J* e J T ( JJ T ) -1 . In 
the redundant case with the pseudoinverse, (4) is a 
particular solution, the minimum norm solution, of the 
general solution given by 

8q = J # 8x + (I-J # J)v (5) 

Here, v e R n is an arbitrary joint space vector used to 
satisfy some criterion such as obstacle avoidance. The 
null space vector , v, is projected into the null space of 
the Jacobian, taking advantage of the redundancy. 
Therefore, the solution given by (5) still satisfies 
8x = J8q . It might be noted that in the non-redundant 
case where J # =J -1 , the second term of equation (5) 
vanishes and the null space vector has no effect. 

2 Singularities and Workspace Boundaries 

The solutions to the inverse kinematics problem 
given in the previous section work well for control of a 
manipulator when it is not near a singularity or 
workspace boundary. However, near a singularity or 
workspace boundary certain components of commanded 
movements either require large joint rates or are 
physically impossible to satisfy. Therefore a robust 
algorithm for the calculation of the inverse kinematics of 
a manipulator must deal with singularities and 
workspace boundaries. 

Physically, a singularity may be described using end 
effector motions (or forces) in the task space. In a 
singular configuration a manipulator is degenerate, 
causing the end effector to loose degrees of freedom in 
the task space. This means that the robot cannot move 
(control forces) in certain directions or that motion 
(force) in some direction is dependent upon motion 
(force) in others. Near a singularity small motions (large 
forces) in certain directions require large joint rates 
(small joint torques). 

Mathematically a singularity may be described using 
the Jacobian matrix. The Jacobian is rank deficient 
(rank(J)<m) when the manipulator is in a singular 
configuration. In the case of m=n the determinate of the 
Jacobian is zero. Near a singularity the Jacobian 
becomes ill-conditioned and elements of the inverse or 
pseudoinverse are large. If the condition number of the 
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Jacobian becomes too large then a general solution 
attempting to solve equation (2) will have numerical 
problems. 

A significant amount of research is devoted to 
developing inverse kinematics with singularity 

robustness. Methods that utilize redundancy to avoid 
singularities have been proposed by many 

researchers. 16,17,18,19 However none of these methods 
ensure both a nonsingular Jacobian and non-cyclic 
behavior. Also, they need the null space vector which 
might be used for other purposes. Whitney 14 proposed 
removing the under generating block of the Jacobian and 
then using a pseudoinverse to calculate an approximate 
solution near singularities. However this requires a 
different Jacobian for each singularity. Futhermore, 
achieving continuity when switching solutions is difficult 
with this method. The most appealing of the proposed 
solutions are those that use damped least squares. 1,2,3,4,5 

Workspace boundaries are the boundaries between 
that space which is reachable by the manipulator 
(W c R m ) and that space which is not (W). These 
boundaries occur in configurations where the 
manipulator is at a singularity(s) or at a joint limit(s). 
Because of joint limits an algorithm that deals with all of 
the singularities of a manipulator does not necessarily 
deal with all of the workspace boundaries. 

Few solutions to the joint limit problem have been 
given in the literature. Some propose the use of 
redundancy to avoid joint limits. 6 The methods proposed 
do not ensure their avoidance and are not applicable in 
cases where redundancy is not available. In practice this 
problem has been dealt with at the global level of path 
planning, searching the entire path for unreachable 
configurations. However, this is not always possible 
such as when the manipulator is operated 
teleoperationally with a human giving real time 
commands. 

3 Inverse Kinematics Using Damped l east Squares 

The damped least squares solutions to the inverse 
kinematics problem are intended to ensure a well 
conditioned matrix for the solution algorithm while 
limiting the size of the solution vector, 8q. This is done 
by adding a diagonal matrix, al, to the matrix JJ T . 
Damped least squares may be used when an approximate 
solution to equation (2) is necessary or acceptable. 


If Sqis found using the equation 8q = J*8x, where 
J* is the pseudoinverse. Then the solution.Sq, satisfies 

min||8q[| 2 (6) 

Sq 

among all Sq satisfying 

min||8x-J6q| 2 (7) 

8q 

where |-|| denotes the Euclidean norm. If the Jacobian is 
of full rank then satisfying the constraint (7) is the same 
as satisfying equation (2). 

If the approximate solution of damped least squares, 
Sq = J + 8x where 

J + =J T (JJ T +air 1 , a>0, (8) 

is used then the solution satisfies 

min{|8x-J8q|| 2 +a 2 ||Sq| 2 } (9) 

8q 

Note that for a=0 the damped least squares solution is 
the pseudoinverse solution. 

In the damped least squares case the size of error in 
the task space is weighed against the size of the resulting 
solution. For a given 8x the size of the solution vector is 
decreased by increasing a. However, this is done at the 
expense of using an approximate solution. As a 
increases so does the size of the error in the task space. 
A large a has the other benefit of ensuring a well 
conditioned matrix for inversion. It has been shown by 
Mayorga et al. 1 that the condition number, K, of the 
matrix P = ( JJ T + al) , is 

K = (a?+a)/(a^+a) (10) 

where CTj > a 2 >...> a m > 0 are the singular values of 
the Jacobian matrix. It can be seen in this equation that 
K is made arbitrarily close to 1 by increasing a, thus 
ensuring a well conditioned matrix for inversion even in 
a singular configuration where a m =0. 

Simply stated, if one is willing to give up the 
exactness of the solution then the size of the joint rates 
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can be reduced and a well conditioned matrix for 
inversion can be ensured by increasing a. It should be 
pointed out that an exact solution may not be physically 
obtainable or even desirable near a singularity. This is 
due to the loss of degrees of freedom in the task space or 
the necessity of large joint rates, which may be 
unachievable or beyond safety limits. 


The weighted damped least squares solution is 
intended to increase the utility of the damped least 
squares solution by providing the ability to significantly 
affect the size of individual components of mimimized 
vectors. This is done by adding weighting matrices to 
the formulation of the damped least squares problem. 


imn{||W x (8x-j8q)f + a 2 |w q 1 8q 


Taking the diagonal structure of the weighting 
matrices into account, it can be seen in the minimization 
condition (16) that the relative importance of the 
individual components of the task space error vector and 
of the solution vector are controlled by the individual 
elements of W x and W q respectively. If a>0 then 
increasing the size of w x . decreases the size of 
(8x- JSq), and decreasing the size of w q . decreases the 
size of 8q,. The strict inequalities in (12) may be 
relaxed for a>0 allowing w q . =0 for some i. This may 
be desired if it is necessary to eliminate the use of some 
Sqj from the solution, such as at a joint limit. 


The importance of individual components in the 
minimization condition of the damped least squares 
solution can be adjusted by scaling the rows and columns 
of the Jacobian before the damped least squares solution 
is calculated. Consider the following reformulation of 
equation (2): 

W x 8x = W x JW q W- 1 8q (11) 

where the weighting matrices are defined by 
W x sdiag[w x ...w ], w x >0 

( 12 ) 

W q =diag[w qi ...Wq n ], w q . >0 

If the following definitions are made 

8x w = W x 8x, 8q w ^W- 1 8q, 

Jw-W x JW q , P wS J w 4 +(X i 

then (1 1) can be written 


5x w — J w 6q v 


In solving the inverse kinematics it is desirable to 
avoid the explicit inversion of a matrix since this is 
computationally expensive. A more efficient algorithm 
will solve a linear system of equations using Guassian 
elimination or some similar method. It is also desirable 
to minimize the number of matrix-matrix multiplies and 
to factor the matrix-matrix and matrix-vector 
multiplication. The following reformulation of the 
problem, will aid in the understanding of the solution 
algorithm presented here. 

8q =W q j;Sx w 

= W q jT(J w jT + 0 J)-l 8 x w 
= W J t P -1 8x ^ ' 

q J w w w 

= W q jTy 
where y = P^Sx* . 

An efficient solution algorithm for inverse 
kinematics problem using weighted damped least 
squares is summarized in the following five steps: 


Solving (14) using damped least squares 
(8q w = Jw(J w Jw +al) _1 8x w = Jw8x w ) results in an 
approximate solution to equation (2), given by 

8q = W q 8q w = W q J*8x w (15) 

This solution satisfies 


1. calculate or set J,8x,W x ,W q , and a 

2. form 8x w = W x 8x and J w = W x JW q 

3. form P w = J w Jw + al 

4. solve 8x w = P w y for y using Cholesky 
decompostion 

5. form 8q = W q j£y 
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It is not claimed that this is the computationally optimal 
algorithm for solving this problem. An optimal 
algorithm is both application and hardware dependent. 
However, this algorithm is fairly efficient if the 
implementation takes advantage of the structures of the 
matrices and vectors involved. Most important are the 
diagonal structure of W x and W q , and the symmetry of 
P w . Additionally, P w is positive definite if: 1) 

rank(J w ) = m and a SO, or 2) a>0. J w can rally 
be rank deficient if J is rank deficient or some weight is 
set to zero. In either of these cases, a will be 
significantly larger than zero if it is set correctly. 
Therefore, Cholesky decomposition can be used for the 
solution of the linear system in step 4. One might note 
that the two conditions given above, are actually the 
conditions that ensure P w is nonsingular, which is 
important in any solution scheme. 

The algorithm above does not include the utilization 
of a null space vector for self motion of redundant 
mani pulators. A algorithm similar to the one above 
which utilizes the a null space vector requires a 
reformulation of the general solution to equation (2). 
Such a reformulation follows. 

8q = W q {j;8x w +(I-j;j w )v w } 

= w q {j>; 1 2 3 4 5 8x w + (i - j^p; 1 w x jw q )w q _1 v} 

= W q {J^p; 1 (8x w - W x Jv) + W-M (18) 

= W q j£p^W x (8x-Jv)+v 

= W q jJy + v 

where y = Pw*W x (8x- Jv) . 

An algorithm similar to the one above which 
implements the general solution is summarized by the 
following five steps. 

1. calculate or set J,8x,v, W x ,W q , and a 

2. form z = W x (8x - Jv) and J w = W x JW q 

3. form P w = J w j£ + al 

4. solve z = P w y for y using Cholesky 
decomposition 

5. form 8q = W q j£y + v 


6 Setting the Weighting Matrices 
and the Damping Factor 

6.1 General Discussion of the Weighting Matrices 
and the Damning Factor 

Developing an algorithm to set the weights and 
damping factor is not a trivial task. There are several 
requirements of this algorithm. The basic idea is to 
dynamically adjust the weights and damping factor so 
that: 1) an exact solution is obtained away from 

singularities and joint limits; 2) an approximate 
solution, that is near the desired solution in the task 
space and is physically realizable in the joint space, is 
obtained near singularities and joint limits; 3) the 
transitions between exact and approximate solutions and 
between different approximate solutions are smooth. 

A non-zero damping factor, a, is used to ensure a 
well conditioned matrix and to include the solution 
vector in the minimized quantity. However, for a>0 the 
solution is an approximate one. Therefore, away from 
singularities and joint limits, a should be zero. As the 
manipulator approaches a singularity or joint limit a 
should be increased in a manner that: 1) ensures a stable 
numerical solution; 2) keeps the joint differentials 
within safe limits. A value of a that satisfies the second 
condition will most probably satisfy the first. It should 
be noted that the first condition is only a concern near 
singularities or when there is a very large relative 
difference in the size of the individual weights that are 
used. It should also be noted that satisfying the second 
condition is dependent upon the method used to set the 
joint weights. 

The relative sizes of the joint weights are used to 
control the importance of each of the joints in the 
minimization. If a joint has a small weight and a is 
non-zero, then the approximate solution tends not to use 
that joint. For example, in a region near a singularity 
where the rate for joint i tends to be large, w q . should be 
small. The weight might also be small near a limit for 
joint i. However, setting a joint weight based solely on 
the distance from the joint limit poses a problem in itself. 
If this is done, the joint will tend to stay at the limit even 
if the exact solution gives a 5q, which is away from the 
limit. Therefore it is necessary to include another 
criterion such as the direction of Sqj in the previous 
iteration of the inverse kinematics. 
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The relative sizes of the error vector weights can be 
used to control the importance of individual components 
of the task space error vector. Increasing w x . decreases 
the size of (8x-J8q)}. While there are certainly 
situations in which it would be useful to dynamically set 
W x , what is suggested here is use of a constant W x to 
normalize the Jacobian matrix. For m=6 the Jacobian 
contains three rows related to position and three rows 
related to orientation. The units in which position and 
orientation are measured create large differences in the 
relative magnitude of the elements of 8x. A constant 
W x can be used to balance the importance of position 
and orientation. 

6.2 Previous Work Dealing With Setting the 

Damping Factor 

Several schemes have been proposed for the 
computation of the damping factor in damped least 
squares without dynamic weighting. Most of these 
schemes are aimed at providing singularity robustness 
and do not include provisions for joint limits. Nakamura 
and Hanafusa 2 proposed that the damping factor be 
calculated as follows: 

fcxoO-w/wo) 2 if w < w 0 
\ 0 if w > w 0 1 ' 


(JJ T + aI) = LDL t . It is shown that the parameters £ , 
d g , and d s can be calculated inexpensively as by- 
products of Guassian elimination in the solution of the 
damped least squares problem. A method was suggested 
by Chan and Lawrence 4 for the calculation of the 
damping factor for both singularity and workspace 
boundary robustness. They give the equation 

a = a 0 ||8x e f (21) 

where 8x e is the error of the manipulator in the task 
space and a 0 is a constant. None of the methods given 
in first three references 1 * 23 provide for a non-zero 
damping factor near joint limits. This is because they 
are established only for singularity robustness. The last 
scheme 4 , which does include provisions for joint limits, 
has questionable performance in providing a well 
conditioned matrix near singularities. Also, its 
performance with load generated task space errors is 
questionable. All of these methods are intended to be 
used throughout the workspace of the manipulator. 
While it is theoretically justified to find a general 
method to handle all cases, perhaps a more effective 
solution would be to find measures which target specific 
singularities and joint limits. Such measures could also 
be used in the setting of the joint weights to specifically 
target the problem joint(s) for that singularity. 


where w = ydet(JJ T ) is Yoshikawa's manipulability 
measure 20 and a 0 and w 0 are constants to be determined 
experimentally. A modification to this scheme is given 
Kelmar and Kholsa 3 . They suggest a method of 
calculating the damping factor using the manipulability 
measures at the ith and (i+l)th iterations. Mayorga et 
al. 1 proposed a scheme which establishes an upper 
bound, ^ 0 , for the condition number of the matrix 
(JJ T +al). In their scheme a is calculated for the next 
iteration using parameters calculated in the present 
iteration and the three constants a 0 , £ 0 , and m. The 
method is described as follows: 


<*i+l = 


ct 0 (i-£/e 0 ) 2 

0 


iff >f 0 
ife<e Q 


( 20 ) 


where £ = m£ 4 d g / d s . Here £ is the upper bound of the 
infinity norm d g and d s are the greatest and 

smallest elements, respectively, of the diagonal matrix 
D, and L is the unit triangular matrix such that 


6.3 Scheme for Setting the Weighting Matrices and 
the Damping Factor 

A scheme for setting the damping factor and joint 
weights is presented here. It considers both singularities 
and joint limits. The equations for this scheme, (22) 
through (29), are given below. In each iteration of the 
kinematic calculations, it determines a damping factor 
and a set of joint weights for each singularity. It also 
determines a damping factor and a joint weight for each 
pair of joint limits. However, only the maximum 
damping factor, and the minimum weight for each joint, 
are used in the inverse kinematics solution (equations 
(28) and (29)). 

The damping factor ranges in value from zero, when 
the manipulator is not in the region of a singularity or 
limit, to a 0 , when the manipulator is at a singularity or 
limit. The joint weights range in value from one, away 
from singularities and limits, to w q0s at a singularity, 
and to w q o( at a joint limit. Different minimum values 
for the joint weights are used at singularities and limits 
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because at a limit it is desired to completely eliminate 
the joint from the solution, while at singularity it might 
only be desired to control the rate of the joint. To 
prevent sticking at the limit, zero is not used. In practice 
a small joint differential obtained using a small joint 
weight, rather than zero, can be discarded. 

For the singularities the damping factor and joint 
weights are set using a measure of distance from the 
singularity (equations (22) and (23)). A measure erf 
distance, either a linear distance or an angle, must be 
identified for each singularity that is reachable by the 
manipulator. The joints that need additional damping 
also need to be identified for each singularity. If it is not 
possible to make these identifications, then (22) might be 
replaced by a scheme similar to those given in (19) or 
(20). However, the benefits of dynamic joint weighting 
are lost, for singularities, if these schemes are used. 


w qs „ ,w* joint weight of joint i as calculated for 

singularity j and for joint limit i 

d Sj .d^ "distance" from singularity j and from 

joint limit i 

\i(. temporal ramp for joint limit i 

SqjoM differential for joint i from previous 

iteration 


Ja 0 (l-(d s /d 0s ) 2 ) lfd Sj <d o Sj 
[ 0 if<V d o Sj 


( 22 ) 


_JW(d s .) if d Sj <d 0s . andieij 
Wqs ii ~{ 1 J if d Sj * d o Sj origA j (23) 
where W(d s .) = w q0s + (l-w q0s )d s . /d 0s . 


fhilim; — qj if8q iold >0 
[q, -lolim; if 8q iold < 0 


Because a joint limit is a one sided problem, 
temporal ramps are used in setting the damping factor 
and weights, when considering joint limits (equations 
(26) and (27)). These ramps are calculated using: one as 
an upper bound; a dynamic lower bound found using the 
distance from the limit the joint is moving towards; and 
a discrete step (equations (24) and (25)). If a joint is in a 
joint limit region and moving towards the limit, then its 
weight is ramped down to a value that is found using the 
distance from limit. If it is moving away from the limit, 
then its weight is ramped back up to 1. 

The following nomenclature and equations are 
defined for the algorithm used to set the damping factor 
and joint weights. 

Constants 

a 0 upper limit of damping factor 

WqOs.WqOt lower limits of joint weights for 

singularities and joint limits 

dos. .doj edge the region for singularity j and 

edge of region for joint limits 

8u step to increase or decrease the joint 

limit ramps in successive iterations 
hilimj .lolinti ....high and low limit for joint i 

Aj set of joints for increased damping near 

singularity j 

Variables 

a s . ,a {< damping factor as calculated for 

singularity j and for joint limit i 


u 'i 


fmax{u^. -8u, d t . /dot) d li <d ot 
j min{Uf. +8u, 1) if d,. >do, 

w <* =w q ot+(l-w q ot)u { . 

a,. =<x 0 (l-u£) 
a = max|max(a s . ), max(a ( . ) j 

w q . = min{ min(w 


l 


nin( w qs „ ), Wq,. j 


(25) 

(26) 

(27) 

(28) 
(29) 


A constant W x is used to normalize the Jacobian 
matrix. The first three rows of the Jacobian are related 
to position, which is measured in linear units, and the 
last three are related to orientation, which is measured in 
radians. The elements of W x are set as follows: 




= NORM 
= 1 


(30) 


Here NORM = jt / (max reach of manipulator) . 

7 Discussion of an Implementation of 
Weighted Damped Least Squares 

Weighted damped least squares, along with the 
scheme for setting the weights and damping factor, is 
used in the control of a slave manipulator in a 
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teleoperational master and slave system. The controller 
for the slave, which runs on a 33 MHz 486 PC/AT at 
about 300 Hz, includes: analog and digital interfaces, 
joint servos, inverse kinematics, and communications 
with the master controller. The telerobotic manipulators 
used in the system are the Kraft master and slave. 
These manipulators have six degrees of freedom and are 
kinematically similar. In the particular control system 
described however, they are not controlled using a joint 
space mapping between the master and slave, which is 
normally true of kinematically similar master and slave 
systems. Instead, a Cartesian space mapping, with 
scaling and indexing, is used. Therefore, the master can 
command unreachable configurations to the slave. 
Furthermore, the master may not be near a singularity, 
and therefore not hindered in any direction, while the 
slave is operating near a singularity, and therefore with 
limited capabilities in certain directions. Weighted 
damped least squares is used in dealing with these 
unreachable configurations and singularities, in real time 
within the local control loop of the slave. Exact 
solutions of equation (2) are found when the slave is 
away from joint limits and singularities, and 
approximate solutions are found when the slave is at or 
near a joint limit or singularity. 

This inverse kinematics algorithm performs quite 
well in the system described above. The human operator 
is free to move the master about without worrying about 
what will happen when the slave is given physically 
unrealizable commands. The approximate solutions are 
both smooth and stable when the manipulator contacts 
the environment and/or when the manipulator is in 
several joint limit and/or singularity regions. Operation 
near a singularity results in approximate solutions with 
damped motion for the joints which swing about 
dangerously if the exact solution is used. Operation near 
joint limits result in approximate solutions that do not 
use the limited joint. The movements of the end effector, 
resulting from the approximate solutions, are intuitive to 
the operator. This is because the solutions are 
approximate in the task space. In contrast, solutions that 
are approximate in the joint space are not intuitive to the 
operator. Approximate joint space solutions result from 
an exact mathematical solution to equation (2) with 
partial implementation in joint space due to the physical 
limitations. 

If the parameters of the algorithm are tuned well, 
the transitions between exact and approximate solutions 


and between different approximate solutions are smooth. 
However, if small values are used for do s . and/or d^ 
then the manipulator tends to "jerk" when transitioning 
from one solution to the next. If a large value is used for 
w q of, then the resulting mathematical solution uses the 
limited joint even at the limit, and the physical solution, 
which is a partial implementation of the mathematical 
solution, is not intuitive. However, if zero is used for 
WqQj then the joint differential does not reverse and 
allow the joint move away from the limit. It was found 
that a small value of w q0( is sufficient to allow this to 
happen. It was also found experimentally that a small 
value for a 0 was sufficient. Although it is not done 
here, a minimum value for a 0 might be developed using 
some theoretical justification such as an upper bound on 
the condition number of (JJ T +al) at the singularities. 

8 Conclusion 

In this paper a general procedure for the calculation 
of inverse kinematics with singularity and joint limit 
robustness has been presented. The procedure uses a 
damped least squares solution to solve the inverse 
kinematics iteratively and incorporates dynamic joint 
weighting for the reduction of specific joint differentials. 
The procedure gives an approximate solution at or near 
singularities and/or joint limits and an exact solution 
away from singularities and joint limits. An algorithm 
was given for the efficient calculation of the inverse 
kinematics using the procedure and a scheme for setting 
damping factor and joint weights was given. The 
algorithm and scheme were implemented for a six d.o.f. 
teleoperator and a well behaved slave manipulator 
resulted under teleoperational control. 
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